package com.googlecode.grs.mechanism;

import java.util.ArrayList;

import com.googlecode.grs.core.GRTObject;
import com.googlecode.grs.core.Mechanism;
import com.googlecode.grs.event.RangeFinderEvent;
import com.googlecode.grs.event.RangeFinderListener;
import com.googlecode.grs.sensor.RangeFinder;

/**
 * Abstracts sensor data to be used for centering the robot between two walls on
 * the path
 * 
 * @author Dan
 * 
 */
public class CenteringRangeFinderArray extends GRTObject implements Mechanism,
		RangeFinderListener {

	ArrayList<CenteringRangeFinderListener> listeners = new ArrayList<CenteringRangeFinderListener>();
	private final RangeFinder right;
	private final RangeFinder left;

	private double lastRight, lastLeft; // readings

	public CenteringRangeFinderArray(RangeFinder left, RangeFinder right) {
		this.left = left;
		this.right = right;

	}

	public void addCenteringRangeFinderListener(
			CenteringRangeFinderListener listener) {
		listeners.add(listener);

	}

	public void removeCenteringRangeFinderListner(
			CenteringRangeFinderListener listener) {
		listeners.remove(listener);

	}

	@Override
	public void pause() {
		// TODO Auto-generated method stub

	}

	@Override
	public void unPause() {
		// TODO Auto-generated method stub

	}

	@Override
	public void halt() {
		// TODO Auto-generated method stub

	}

	public void startListening() {
		left.addRangeFinderListener(this);
		right.addRangeFinderListener(this);

	}

	public void stopListening() {
		left.removeRangeFinderListener(this);
		right.removeRangeFinderListener(this);

	}

	/**
	 * Called by a RangeFinder
	 * 
	 * @see RangeFinder
	 */
	public void distanceChanged(RangeFinderEvent s) {
		if (s.getSource() == left) {
			lastLeft = s.getFinalDistance();
		} else if (s.getSource() == right) {
			lastRight = s.getFinalDistance();
		}
		notifyOnFurthestWallChange(lastLeft > lastRight ? CenteringWall.LEFT
				: CenteringWall.RIGHT);
	}

	private void notifyOnFurthestWallChange(CenteringWall newWall) {
		log("Furthest wall:" + newWall);
		for (CenteringRangeFinderListener listener : listeners) {
			listener.furthestWallChanged(newWall);
		}
	}

}
